package com.lyfam.game.behaviosr.steering;

import processing.core.PApplet;
import processing.core.PGraphics;

import com.lyfam.game.entity.MovingEntity;
import com.lyfam.game.geometry.Vector2D;

public class WanderingBehavior implements ISteeringBehavior
{
	public int compareTo(ISteeringBehavior o)
	{
		if (getPriority() == o.getPriority())
		{
			return 0;
		}
		else
		{
			if (getPriority() >o.getPriority())
			{
				return 1;
			}
			else
			{
				return -1;
			}
		}
	}

	public static final float DEFAULT_RADIUS = 10f;
	public static final float DEFAULT_DISTANCE = 30f;
	public static final float DEFAULT_JITTER = 5.67f;
	
	private float wanderRadius;
	private float wanderDistance;
	private float wanderJitter;
	private boolean showWanderingCircle;
	
	private Vector2D wanderTarget;	
	
	private PGraphics g;
	private MovingEntity vehicle;
	
	/**
	 * Create a wandering behavior with the default radius, distance and jitter rate
	 * 
	 * @param move The vehicle
	 * @param p Usually PGraphic g of PApplet
	 */
	public WanderingBehavior(MovingEntity move, PGraphics p)
	{
		this(move, p, DEFAULT_RADIUS,DEFAULT_DISTANCE, DEFAULT_JITTER);
	}
	
	/**
	 * Create a wandering behavior
	 * 
	 * @param move Vehicle
	 * @param g PGraphic
	 * @param radius The wandering radius
	 * @param distance The distance from vehicle position to the center of the wandering circle
	 * @param jitterRate Jitter rate
	 */
	public WanderingBehavior(MovingEntity move, PGraphics g, float radius, float distance, float jitterRate)
	{
		this.vehicle = move;
		this.g = g;
		this.wanderRadius = radius;
		this.wanderDistance = distance;
		this.wanderJitter = jitterRate;
		
		double theta = PApplet.radians(360) * getRandom();
		this.wanderTarget = new Vector2D(
				(float) Math.cos(theta) * wanderRadius, (float) Math.sin(theta)
						* wanderRadius);
	}

	private Vector2D calculateSteeringForce()
	{
		wanderTarget.add(new Vector2D(getRandom() * wanderJitter, getRandom()
				* wanderJitter));
		
		wanderTarget.normalize();
		wanderTarget.mult(wanderRadius);
		Vector2D targetForce = Vector2D.clone(wanderTarget);
		
		float theta = vehicle.getVelocity().heading2D();
		
		/**
		 * Affine transform:
		 * - Translate the origin to vehicle position
		 * - rotate so that the X axis point to vehicle heading (velocity)
		 * - translate the coordinate again to move wandering circle to the preset distance
		 *  in front of vehicle
		 */
		g.pushMatrix();
		g.translate(vehicle.getPosition().getX(), vehicle.getPosition().getY());
		g.rotate(theta);
		// I do this instead of adding the distance to the unit cirle of
		// wandering
		g.translate(wanderDistance, 0);
		
		float worldX = g.screenX(targetForce.getX(), targetForce.getY());
		float worldY = g.screenY(targetForce.getX(), targetForce.getY());
		
		// show the circle of wander
		if (showWanderingCircle)
		{
			g.noFill();
			g.line(0, 0, targetForce.getX(), targetForce.getY());
			g.ellipse(0, 0, wanderRadius*2, wanderRadius*2);			
		}
		
		g.popMatrix();
		// return to original coordinate
		
		Vector2D worldForce = new Vector2D(worldX, worldY);
		worldForce.sub(vehicle.getPosition());
		
		return worldForce;
	}

	public Vector2D getSteeringForce()
	{
		return this.calculateSteeringForce();
	}

	public float getRandom()
	{
		return (float) (2 * Math.random() - 1);
	}

	public boolean isShowWanderingCircle()
	{
		return showWanderingCircle;
	}

	public void setShowWanderingCircle(boolean showWanderingCircle)
	{
		this.showWanderingCircle = showWanderingCircle;
	}

	public int getPriority()
	{
		return 5;
	}
}
